
#include "slepdef.h"
#include "slepblas.h"
#include <string.h>
#include <math.h>
#include <assert.h>
#include <malloc.h>
#include "slepmacros.h"

/* Assumes 'dest' already has memory allocated to hold vector of size of orig's */
void slep_dvcopy(slep_vector* orig, slep_vector* dest){
	static int i=0;
	if(orig->sparsity == SlepSparse){
		dest->sparsity = orig->sparsity;
		dest->dim = orig->dim ;
		dest->data.spvctr.nnz = orig->data.spvctr.nnz;
		memcpy(dest->data.spvctr.index , orig->data.spvctr.index , sizeof(int)*(dest->data.spvctr.nnz) );
		memcpy(dest->data.spvctr.values , orig->data.spvctr.values , sizeof(double)*(dest->data.spvctr.nnz) );
		return ;
	}else if( orig->sparsity == SlepDense){
		dest->sparsity = orig->sparsity;
		dest->dim = orig->dim;
		dest->inc = 1;

		if( orig->inc == 1)
			memcpy(dest->data.vctr , orig->data.vctr, sizeof(double) * (dest->dim) );
		else if( orig->inc > 1)
			for(i=0;i < orig->dim; i++)
				dest->data.vctr[i] =  orig->data.vctr[i* (orig->inc) ];
		return;
	}

	// TODO have to check and correct for betterness
}

/* Assumes 'dest' already has memory allocated to hold matrix of size of orig's */
void slep_dmcopy(slep_matrix* orig, slep_matrix* dest){
	int nnz = 0; orig->data.spmtrx.Index;

	nnz = findNnz(orig);
	dest->kl = orig->kl;
	dest->ku = orig->ku;
	dest->ld = orig->ld;
	dest->m = orig->m;
	dest->n = orig->n;
	dest->sparsity = orig->sparsity;
	dest->uplobanded = orig->uplobanded;

	if(orig->sparsity == SlepSparse){
		dest->data.spmtrx.format = orig->data.spmtrx.format;
		memcpy(dest->data.spmtrx.ColumnsORRows , orig->data.spmtrx.ColumnsORRows , sizeof(int) * nnz);
		memcpy(dest->data.spmtrx.values , orig->data.spmtrx.values , sizeof(double) * nnz );
		if(orig->data.spmtrx.format == SlepCompressedSparseColumn)
			memcpy(dest->data.spmtrx.Index , orig->data.spmtrx.Index , sizeof(double) * (orig->n) );
		else if(orig->data.spmtrx.format == SlepCompressedSparseRow)
			memcpy(dest->data.spmtrx.Index , orig->data.spmtrx.Index , sizeof(double) * (orig->m) );
	}else if( orig->sparsity == SlepDense){
		memcpy(dest->data.mtrx, orig->data.mtrx , sizeof(double) * (orig->ld) * (orig->n) ) ; 
	}
	// TODO : else if conditions for sparsity = SlepSymmetric and SlepTriangular
}

slep_vector* slepCopyToNewVector(slep_vector* vctr){
	int i;
	slep_vector* vctr2 = NULL;
	if(vctr->sparsity == SlepSparse){
		vctr2 = (slep_vector*) malloc( sizeof(slep_vector));
		vctr2->sparsity = SlepSparse;
		vctr2->dim = vctr->dim;
		vctr2->data.spvctr.nnz = vctr->data.spvctr.nnz ;
		vctr2->data.spvctr.index = (int *)malloc( sizeof(int) * (vctr->data.spvctr.nnz) );
		vctr2->data.spvctr.values = (double *)malloc( sizeof(double) * (vctr->data.spvctr.nnz) );
		memcpy(vctr2->data.spvctr.index ,  vctr->data.spvctr.index, sizeof(int) * (vctr->data.spvctr.nnz) );
		memcpy(vctr2->data.spvctr.values , vctr->data.spvctr.values, sizeof(double) * (vctr->data.spvctr.nnz) );
		return vctr2;
	} else if( vctr->sparsity == SlepDense){
		vctr2 = (slep_vector*) malloc( sizeof(slep_vector));
		vctr2->sparsity = SlepDense;
		vctr2->dim = vctr->dim;
		vctr2->inc = 1;
		vctr2->data.vctr = (double *) malloc( sizeof(double) * (vctr->dim)  );
		if( vctr->inc == 1)
			memcpy(vctr2->data.vctr, vctr->data.vctr , sizeof(double) * (vctr->dim) );
		else if( vctr->inc > 1)
			for(i=0;i < vctr->dim; i++)
				vctr2->data.vctr[i] =  vctr->data.vctr[i* (vctr->inc) ];
		return vctr2;
	}
	return vctr2;
}



slep_matrix* slepCopyToNewMatrix(slep_matrix* mtrx){
	slep_matrix * mtrx2 = NULL;
	int nnz=0;
	if(mtrx->sparsity == SlepSparse) {
		mtrx2 = (slep_matrix*) malloc( sizeof(slep_matrix) );
		(*mtrx2) = (*mtrx);
		nnz = findNnz(mtrx);
		mtrx2->data.spmtrx.ColumnsORRows = (int *) malloc( sizeof(int) * nnz );
		mtrx2->data.spmtrx.values = (double *) malloc( sizeof(double) * nnz );
		if(mtrx->data.spmtrx.format == SlepCompressedSparseColumn){
			mtrx2->data.spmtrx.Index = (int *)malloc( sizeof(int) * mtrx->n );
			memcpy(mtrx2->data.spmtrx.Index , mtrx->data.spmtrx.Index , sizeof(double) * mtrx->n );
		}else if(mtrx->data.spmtrx.format == SlepCompressedSparseRow){
			mtrx2->data.spmtrx.Index = (int *)malloc( sizeof(int) * mtrx->m );
			memcpy(mtrx2->data.spmtrx.Index , mtrx->data.spmtrx.Index , sizeof(double) * mtrx->m );
		}
		memcpy(mtrx2->data.spmtrx.ColumnsORRows , mtrx->data.spmtrx.ColumnsORRows , sizeof(int) * nnz);
		memcpy(mtrx2->data.spmtrx.values , mtrx->data.spmtrx.values , sizeof(double) * nnz );
		return mtrx2;
	}else if(mtrx->sparsity == SlepDense){
		mtrx2 = (slep_matrix*) malloc( sizeof(slep_matrix) );
		(*mtrx2) = (*mtrx);
		mtrx2->data.mtrx = (double *) malloc( sizeof(double) * (mtrx2->ld) * (mtrx2->n) );
		memcpy( mtrx2->data.mtrx, mtrx->data.mtrx , sizeof(double) * (mtrx2->ld) * (mtrx2->n) );
		return mtrx2;
	}
	// TODO : finish for SlepSymmetric and SlepTriangular
	return mtrx2;
}



void slep_dscal(double alpha, slep_vector *x){
	static int i=0,tempi1;

	if (alpha==1)
		return;

	if (alpha==0. && x->sparsity == SlepDense && x->dim == 1){
		DMEMSET(x->data.vctr,0, x->dim ,i);
		return;
	}

	if (alpha==0. && x->sparsity == SlepSparse ){
		DMEMSET(x->data.spvctr.values,0, x->data.spvctr.nnz ,i);
		return;
	}

	if(x->sparsity == SlepSparse)
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			(x->data.spvctr.values[i]) *= alpha;
	else if(x->sparsity == SlepDense)
		for(i=0,tempi1=(x->dim)*(x->inc); i < tempi1 ; i+=(x->inc))
			(x->data.vctr[i]) *= alpha;
}

void slep_daxpy(double alpha, slep_vector* x, slep_vector* y){
	static int i,j;
	static int temp=0;
	if(alpha==0)
		return;
	if(x->sparsity == SlepDense && y->sparsity == SlepDense){
		temp = (x->dim) * (x->inc);
		if(x->dim == y->dim)
			for(i=0,j=0; i < temp ; i+=(x->inc), j+=(y->inc) )
				(y->data.vctr[j]) += alpha * (x->data.vctr[i]);
		return;
	}
	else if( x->sparsity == SlepSparse && y->sparsity == SlepSparse ){//		if( (x->data.spvctr.nnz) == (y->data.spvctr.nnz) )
		for(i=0,j=0; (i < (x->data.spvctr.nnz)) && (j < (y->data.spvctr.nnz)) ; ){
			if( (x->data.spvctr.index[i]) == (y->data.spvctr.index[j]) )
				((y->data.spvctr.values[j]) += alpha*(x->data.spvctr.values[i])), i++,j++ ;
			else if( (x->data.spvctr.index[i]) < (y->data.spvctr.index[j]) )
				i++;
			else
				j++;
		}
	}
	// TODO for mixed sparsity ( may not be feasible)

}


void slep_daxpby(double alpha, slep_vector* x, double beta, slep_vector* y){
	int i, j;
	int temp = 0;
	if(alpha==0)
		return;
	if(x->sparsity == SlepDense && y->sparsity == SlepDense){
		temp = (x->dim) * (x->inc);
		if(x->dim == y->dim)
			for(i=0,j=0; i < temp ; i+=(x->inc), j+=(y->inc) )
				(y->data.vctr[j]) *= beta, (y->data.vctr[j]) += alpha * (x->data.vctr[i]);
		return;
	}
	else if( x->sparsity == SlepSparse && y->sparsity == SlepSparse ){ //		if( (x->data.spvctr.nnz) == (y->data.spvctr.nnz) )
		for(i=0,j=0; (i < (x->data.spvctr.nnz)) && (j < (y->data.spvctr.nnz)) ; ){
			if( (x->data.spvctr.index[i]) == (y->data.spvctr.index[j]) )
				(y->data.spvctr.values[j]) *= beta , ((y->data.spvctr.values[j]) += alpha*(x->data.spvctr.values[i])), i++,j++ ;
			else if( (x->data.spvctr.index[i]) < (y->data.spvctr.index[j]) )
				i++;
			else
				j++;
		}
	}
	// TODO for mixed sparsity ( may not be feasible)
}

double slep_ddot(slep_vector *x, slep_vector *y){
	static double dotproduct=0;
	static int i,j, tempi1, tempi2;
	dotproduct=0;
	if(x->sparsity == SlepDense && y->sparsity == SlepDense){
		if(x->inc==1 && y->inc==1 && x->dim == y->dim){
			for(i=0; i < x->dim ; i++)
				dotproduct += (y->data.vctr[i]) * (x->data.vctr[i]) ;
			return dotproduct;
		}
		assert(x->dim == y->dim);
		tempi1 = (x->dim) * (x->inc);
		tempi2 = (y->dim) * (y->inc);
		for(i=0,j=0; i < tempi1 ; i+=x->inc, j+=y->inc)
			dotproduct += (y->data.vctr[j]) * (x->data.vctr[i]) ;
		
	}
	else if( x->sparsity == SlepSparse && y->sparsity == SlepSparse )//		if( (x->data.spvctr.nnz) == (y->data.spvctr.nnz) )
		for(i=0,j=0; (i < (x->data.spvctr.nnz)) && (j < (y->data.spvctr.nnz)) ; ){
			if( (x->data.spvctr.index[i]) == (y->data.spvctr.index[j]) )
				(dotproduct += (y->data.vctr[j]) * (x->data.vctr[i]) ), i++,j++ ;
			else if( (x->data.spvctr.index[i]) < (y->data.spvctr.index[j]) )
				i++;
			else
				j++;
		}
	// TODO for mixed sparsity ( [x-sparse,y-dense] and [x-dense,y-sparse] )
		// Completed but have to check for correctness
	else if( x->sparsity == SlepSparse && y->sparsity == SlepDense ){
		assert( (x->data.spvctr.index[(x->data.spvctr.nnz)-1]) <= y->dim );
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			dotproduct += (x->data.spvctr.values[i]) * (y->data.vctr[ (x->data.spvctr.index[i])* (y->inc) ]);
	}
	else if( x->sparsity == SlepDense && y->sparsity == SlepSparse ){
		assert( (y->data.spvctr.index[(y->data.spvctr.nnz)-1]) <= x->dim );
		for(i=0; i < (y->data.spvctr.nnz) ; i++)
			dotproduct += (y->data.spvctr.values[i]) * (x->data.vctr[ (y->data.spvctr.index[i])* (x->inc) ]);
	}

	return dotproduct;
}

double slep_dnrm2(slep_vector* x){
	static double norm=0;
	static int i=0;
	norm=0;
	if(x->sparsity == SlepDense){
		for(i=0; i < (x->dim) ; i++)
			norm += (x->data.vctr[i*(x->inc)]) * (x->data.vctr[i*(x->inc)]) ;
		return sqrt(norm);
	}else if( x->sparsity == SlepSparse){
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			norm += (x->data.spvctr.values[i]) * (x->data.spvctr.values[i]) ;
		return sqrt(norm);
	}
	return 0;
}


double slep_dpnrm(slep_vector* x, double p){
	double norm=0;
	int i=0;

	if(p > 1e20)
		return slep_damax(x);
	if( p<1.01 && p >= 0.99)
		return slep_dasum(x);
	if( p<2.01 && p >= 1.99)
		return slep_dnrm2(x);
	norm=0;
	if(x->sparsity == SlepDense){
		for(i=0; i < (x->dim) ; i++)
			norm += pow((x->data.vctr[i*(x->inc)]) , p);
		return pow(norm, 1.0/p);
	}else if( x->sparsity == SlepSparse){
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			norm += pow((x->data.spvctr.values[i]),p);
		return pow(norm, 1.0/p);
	}
	return 0.0;
}


double slep_dasum(slep_vector* x){
	static double norm=0;
	static int i=0;
	norm=0;

	if(x->sparsity == SlepDense){
		for(i=0; i < (x->dim) ; i++)
			norm += fabs( x->data.vctr[i*(x->inc)] )  ;
		return (norm);
	}else if( x->sparsity == SlepSparse){
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			norm += fabs(x->data.spvctr.values[i]);
		return (norm);
	}
	return (norm);
}


double slep_dsum(slep_vector* x){
	static double sum=0;
	static int i=0, tmp1;
	sum=0;

	if(x->sparsity == SlepDense){
		tmp1 = (x->dim) * (x->inc);
		for(i=0; i < (x->dim) ; i+=(x->inc))
			sum += ( x->data.vctr[i] )  ;
		return (sum);
	}else if( x->sparsity == SlepSparse){
		tmp1 = (x->dim) * (x->inc);
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			sum += (x->data.spvctr.values[i]);
		return (sum);
	}
	return (sum);
}


int slep_idamax(slep_vector* x){
	static int saved=0;
	static int i=0;
	if(x->sparsity == SlepDense)
		for(i=0; i < (x->dim) ; i++ )
			if( fabs(x->data.vctr[i*(x->inc)])  > fabs(x->data.vctr[saved*(x->inc)]))
				saved = i;
	else if( x->sparsity == SlepSparse)
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			if( fabs(x->data.spvctr.values[i])  > fabs(x->data.spvctr.values[saved]))
				saved = i;
	return saved;
}

double slep_damax(slep_vector* x){
	static int saved=0, k;
	static int i=0;
	if(x->sparsity == SlepDense){
		for(i=0, k= (x->dim)*(x->inc); i < k ; i+=(x->inc) )
			if( fabs(x->data.vctr[i])  > fabs(x->data.vctr[saved*(x->inc)]))
				saved = i;
		return fabs(x->data.vctr[saved]);
	}
	else if( x->sparsity == SlepSparse){
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			if( fabs(x->data.spvctr.values[i])  > fabs(x->data.spvctr.values[saved]))
				saved = i;
		return fabs(x->data.spvctr.values[saved]);
	}
	return 0.0;
}


double slep_dmax(slep_vector* x){
	static int saved=0, k;
	static int i=0;
	if(x->sparsity == SlepDense){
		for(i=0, k= (x->dim)*(x->inc); i < k ; i+=(x->inc) )
			if( (x->data.vctr[i])  > (x->data.vctr[saved*(x->inc)]))
				saved = i;
		return (x->data.vctr[saved]);
	}
	else if( x->sparsity == SlepSparse){
		for(i=0; i < (x->data.spvctr.nnz) ; i++)
			if( (x->data.spvctr.values[i])  > (x->data.spvctr.values[saved]))
				saved = i;
		return (x->data.spvctr.values[saved]);
	}
	return 0.0;
}


double slep_dfrbnrm(slep_matrix* a){
	int i=0, j=0, ld;
	double frobenius = 0;
	frobenius = 0;
	if(a->sparsity == SlepDense){
		if(a->ld > a->m )
			ld = a->ld;
		else 
			ld = a->m;
		for(i=0; i < a->m ; i++)
			for(j=0; j < a->n ; j++)
				frobenius += (a->data.mtrx[j*ld + i]) * (a->data.mtrx[j*ld + i]);
		
		return sqrt(frobenius);
	}
	else if( a->sparsity == SlepSparse ){
		if(a->data.spmtrx.format == SlepCompressedSparseColumn )
			for(i=0; i < a->data.spmtrx.Index[a->n] ; i++)
				frobenius += (a->data.spmtrx.values[i]) * (a->data.spmtrx.values[i]);
		else if(a->data.spmtrx.format == SlepCompressedSparseRow )
			for(i=0; i < a->data.spmtrx.Index[a->m] ; i++)
				frobenius += (a->data.spmtrx.values[i]) * (a->data.spmtrx.values[i]);
		return sqrt(frobenius);
	}
	return frobenius;
}


double slep_dtraceAB(slep_transposity transA, slep_matrix* a, slep_transposity transB, slep_matrix* b){
	int i,j, temp1;
	double result;
	// TODO finish this procedure
	if(transA == SlepNoTranspose && transB == SlepTranspose){
		result = 0;
		temp1 = (a->m * a->n);
		if(a->ld == a->m && b->ld == b->m)
			for(i=0; i< temp1 ; ++i)
				result += a->data.mtrx[i] * b->data.mtrx[i];
		else
			for(i=0; i< a->n ;++i)
				for(j=0; j < a->m ;++j)
					result += a->data.mtrx[i * a->ld+j] * b->data.mtrx[i * b->ld + j];
		return result;
	}else if(transA == SlepTranspose && transB == SlepNoTranspose ){
		result = 0;
		temp1 = (a->m * a->n);
		if(a->ld == a->m && b->ld == b->m)
			for(i=0; i< temp1 ; ++i)
				result += a->data.mtrx[i] * b->data.mtrx[i];
		else
			for(i=0; i< a->n ;++i)
				for(j=0; j < a->m ;++j)
					result += a->data.mtrx[i * a->ld+j] * b->data.mtrx[i * b->ld + j];
		return result;
	}
	return 0;
}




/* [y = x.diag(y) (or diag(x).y as both are same)] */
void slep_xdiagy(slep_vector* x, slep_vector* y){
	static int i,j,k;
	if( x->sparsity == SlepDense && y->sparsity == SlepDense){
		assert(x->dim == y->dim);
		k = (x->dim) * (x->inc);
		for(i=0,j=0; j<k; i+=y->inc, j+= x->inc)
			y->data.vctr[i] *= x->data.vctr[j];
		return;
	}

	if( x->sparsity== SlepSparse && y->sparsity == SlepSparse){
		for(i=0,j=0; 
			i <= x->data.spvctr.index[x->data.spvctr.nnz - 1] &&
				j <= y->data.spvctr.index[y->data.spvctr.nnz -1]; 
			)
			if( x->data.spvctr.index[i] == y->data.spvctr.index[j]){
					y->data.spvctr.values[j] *= x->data.spvctr.values[i] ;
					i++;
					j++;
			}
			else if( x->data.spvctr.index[i] > y->data.spvctr.index[j]){
				y->data.spvctr.values[j]=0;
				j++;
			}
			else
				i++;
		if( (j <= y->data.spvctr.index[y->data.spvctr.nnz -1]))
			for(; j <= y->data.spvctr.index[y->data.spvctr.nnz -1]; j++)
				y->data.spvctr.values[j]=0;
		return;
	}

	if( x->sparsity == SlepSparse && y->sparsity == SlepDense){
		assert(y->dim >= (x->data.spvctr.index[x->data.spvctr.nnz -1]+1) );
		for(i=0,j=0; (i <= x->data.spvctr.index[x->data.spvctr.nnz -1]) ; i++){
			y->data.vctr[x->data.spvctr.index[i]] *= x->data.spvctr.values[i];
			if ( x->data.spvctr.index[i] > j){
				memset( &(y->data.vctr[j]), 0, sizeof(double) * (x->data.spvctr.index[i] - j) );
				j = x->data.spvctr.index[i] + 1;
			}
		}
		if ( y->dim > j)
			memset( &(y->data.vctr[j]), 0, sizeof(double) * (y->dim - j) );
		return;
	}

	if( x->sparsity == SlepDense && y->sparsity == SlepSparse){
		assert( (y->data.spvctr.index[y->data.spvctr.nnz - 1]+1) <= x->dim);
		for(i=0; i < y->data.spvctr.nnz ; i++)
			y->data.spvctr.values[i] *= x->data.vctr[y->data.spvctr.index[i]];
		return;
	}
	// Completed all the cases
}



/* [y = {diag(x)^-1}.y ] */
void slep_invdiagxy(slep_vector* x, slep_vector* y){
	static int i,j,k;
	assert(x->sparsity != SlepSparse);
	if( x->sparsity == SlepDense && y->sparsity == SlepDense){
		assert(x->dim == y->dim);
		k = (x->dim) * (x->inc);
		for(i=0,j=0; i<k; i+=y->inc, j+= x->inc)
			y->data.vctr[i] /= x->data.vctr[j];
		return;
	}

	if( x->sparsity == SlepDense && y->sparsity == SlepSparse){
		assert( (y->data.spvctr.index[y->data.spvctr.nnz - 1]+1) <= x->dim);
		for(i=0; i < y->data.spvctr.nnz ; i++)
			y->data.spvctr.values[i] /= x->data.vctr[y->data.spvctr.index[i]];
		return;
	}
}


void slep_zerom(slep_matrix *mtrx){
	int i;
	DMEMSET(mtrx->data.mtrx, 0, (mtrx->m) * (mtrx->n) , i);
}

void slep_zerov(slep_vector *vctr){
	int i;
	if( vctr->sparsity == SlepDense){
		DMEMSET( vctr->data.vctr, 0, (vctr->dim), i );
		return;
	}else {
		DMEMSET( vctr->data.spvctr.values, 0, (vctr->data.spvctr.nnz), i );
		return;
	}
	return;
}